//
// Created by Pulsar on 2020/5/16.
//
#include <rc_mapping/slam_devices.h>
#include <iostream>
#include <signal.h>

using namespace RC::Map;

void call_function(std::vector <DOT> dots) {
//    for (int i = 0; i < dots.size(); i++) {
//        std::cout << "dist: " << dots[i].dist << " theta: " << dots[i].theta << std::endl;
//    }
}

bool ctrl_c_pressed;

void ctrlc(int) {
    ctrl_c_pressed = true;
    std::cout<<"Exit"<<std::endl;
}

int main(int argc, char **argv) {
    signal(SIGINT, ctrlc);
    SlamDevice slamDevice("/dev/ttyUSB0");
    slamDevice.bind(call_function);
    slamDevice.start(0, 1);
    while(!ctrl_c_pressed){
        slamDevice.recive();
    }
    slamDevice.stop();
    slamDevice.release();
    return 1;
}